#!/usr/bin/python

from time import sleep
import serial
import struct

# This script needs pySerial from http://pyserial.sourceforge.net/pyserial.html

def add_bt_header(s):
    return struct.pack("BB", len(s), 0) + s

def create_playtone_struct(frequency, duration):
    return struct.pack("BBHH", 0x80, 0x03, frequency, duration)

def create_setoutputstate_struct(output_port, power_set_point, mode_byte,
                                 regulation_mode, turn_ratio, runs_state,
                                 tacho_limit):
    # TODO: Check (and document) ranges etc (from lego docs)
    return struct.pack("BBBBBBbBL", 0x80, 0x04, output_port, power_set_point,
                                    mode_byte, regulation_mode, turn_ratio,
                                    runs_state, tacho_limit)

if __name__=='__main__':
    ser = serial.Serial()
    ser.port = "COM4"    
    try:
        ser.open()
        ser.timeout = 1

        # run motor 1:
        ser.write(add_bt_header(create_setoutputstate_struct(1, 100, 1, 0, 100, 0x20, 1000)))

        # beep at 440hz for 1s
        ser.write(add_bt_header(create_playtone_struct(440, 1000)))

    finally:
        ser.close()
